#include <public/vehicle_iface_v1.h>

// ***********************************************************************

// class EDROOMTopContext

// ***********************************************************************



	// CONSTRUCTORES***********************************************

Vehicle::EDROOMTopContext::EDROOMTopContext (Vehicle &act, TEDROOMBool & EDROOMpVarVErrorInConfig, CDLocation & EDROOMpVarVTargetLocation, CEDROOMPOOLCDLocation & EDROOMpPoolCDLocation ):

	EDROOMcomponent(act)
	,Msg(EDROOMcomponent.Msg)
	,MsgBack(EDROOMcomponent.MsgBack)
	,PathPlannerCtrl(EDROOMcomponent.PathPlannerCtrl)
	,PathTrackerCtrl(EDROOMcomponent.PathTrackerCtrl)
	,AvoidObstaclesSysCtrl(EDROOMcomponent.AvoidObstaclesSysCtrl)
	,Timer(EDROOMcomponent.Timer)
	, VErrorInConfig ( EDROOMpVarVErrorInConfig )
	, VTargetLocation ( EDROOMpVarVTargetLocation )
	, EDROOMPoolCDLocation ( EDROOMpPoolCDLocation )
 {

}

Vehicle::EDROOMTopContext::EDROOMTopContext ( EDROOMTopContext &contex ):

	EDROOMcomponent(contex.EDROOMcomponent),
	Msg(contex.Msg),
	MsgBack(contex.MsgBack)
	,PathPlannerCtrl(contex.PathPlannerCtrl)
	,PathTrackerCtrl(contex.PathTrackerCtrl)
	,AvoidObstaclesSysCtrl(contex.AvoidObstaclesSysCtrl)
	,Timer(contex.Timer)
	, VErrorInConfig ( contex.VErrorInConfig )
	, VTargetLocation ( contex.VTargetLocation )
	, EDROOMPoolCDLocation ( contex.EDROOMPoolCDLocation )
 {

}

	// EDROOMSearchContextTrans********************************************

bool Vehicle::EDROOMTopContext::EDROOMSearchContextTrans(TEDROOMTransId &edroomCurrentTrans){

	bool edroomValidMsg=false; 

	 switch(Msg->signal){

		 case (EDROOMSignalTimeout): 

				if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId= ReStart;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

			 break;

		 case ( EDROOMSignalDestroy ): 

			 edroomValidMsg=true;
			 edroomCurrentTrans.distanceToContext = 0 ;
			 edroomCurrentTrans.localId = -1 ;
			 break;

	}

	return(edroomValidMsg);

}

	// Component Methods  ****************************

void	Vehicle::EDROOMTopContext::FAvObstaclesError()

{
 
 	print_string("error in Avoid Obstacles System\n\r");
 
  
 
}



void	Vehicle::EDROOMTopContext::FConfigAvObstacles()

{
 	
		// invoke 
	
	MsgBack=AvoidObstaclesSysCtrl.invoke(SConfigAO, EDROOMprioVeryHigh);
	
		// Handling MsgBack
	
	VErrorInConfig=(MsgBack->signal==SConfigError);
	
	
		// End Handling MsgBack
	
	EDROOMFreeMsgBack(MsgBack);
 
 
 
 
}



void	Vehicle::EDROOMTopContext::FConfigPathPlanner()

{
 
 
 
	CDLocation * ap_CDLocation = EDROOMPoolCDLocation.AllocData();
	
		// Complete Data 
	
	*ap_CDLocation=VTargetLocation;
	
		// invoke 
	
	MsgBack=PathPlannerCtrl.invoke(SConfigPathPlanner, EDROOMprioVeryHigh, ap_CDLocation, &EDROOMPoolCDLocation);
	
		// Handling MsgBack
	
	
	
	
	VErrorInConfig=(MsgBack->signal==SConfigError);
 
	
		// End Handling MsgBack
	
	EDROOMFreeMsgBack(MsgBack);
	
 
 
 
}



void	Vehicle::EDROOMTopContext::FConfigPathTracker()

{
 
 
		// invoke 
	
	MsgBack=PathTrackerCtrl.invoke(SConfigPathTracker, EDROOMprioVeryHigh);
	
		// Handling MsgBack
	
	VErrorInConfig=(MsgBack->signal==SConfigError);
	
	
		// End Handling MsgBack
	
	EDROOMFreeMsgBack(MsgBack);
 
 
 
}



void	Vehicle::EDROOMTopContext::FHandleObstacle()

{
 
		// send new Msg
	
	PathTrackerCtrl.send(SStopTracking, EDROOMprioVeryHigh);
 
 
	CDLocation * ap_CDLocation = EDROOMPoolCDLocation.AllocData();
	
		// Complete Data 
	
	*ap_CDLocation=sensors_get_current_location();
	
		// send new Msg
	
	PathPlannerCtrl.send(SMarkObstacle, EDROOMprioVeryHigh, ap_CDLocation, &EDROOMPoolCDLocation);
 
 
}



void	Vehicle::EDROOMTopContext::FPathPlannerError()

{
 
 print_string("Error in Planner Config\n\r");
 
 
 
}



void	Vehicle::EDROOMTopContext::FPathTrackerError()

{
 
 print_string("Error in Path Tracker Config\n\r");
 
 
 
}



void	Vehicle::EDROOMTopContext::FProgNextConfig()

{
	
	Pr_Time	interval(0); // Declare time var with an initial value in seconds
	 
	Timer.InformIn( interval, EDROOMprioNormal );
 
 
}



void	Vehicle::EDROOMTopContext::FRecovery()

{
 
 	Pr_Time time;
 
	print_string("Power off all subsystems\n\r");
 
	print_string("Restart in 10 seconds\n\r");
	print_string("----------------------------------\n\r"); 
	// Timing Service 
	 
	time.Add(10);  // seconds
	 
	Timer.InformAt( time, EDROOMprioNormal );
 
 
 
 
}



void	Vehicle::EDROOMTopContext::FStart()

{
 
	
		
 
		// send new Msg
	
	AvoidObstaclesSysCtrl.send(SStart, EDROOMprioVeryHigh);
 
	// send new Msg
	
	PathTrackerCtrl.send(SStartTracking, EDROOMprioVeryHigh);
	
 	// send new Msg
	
	PathPlannerCtrl.send(SStartPlanning, EDROOMprioVeryHigh);
 
 
}



void	Vehicle::EDROOMTopContext::FStartUp()

{
 
	VTargetLocation=sensors_get_current_location();
 
	if(VTargetLocation.latitude>0) 
		VTargetLocation.latitude-=0.1;
	else
		VTargetLocation.latitude+=0.1;
 
	if(VTargetLocation.longitude>0) 
		VTargetLocation.longitude-=0.1;
	else
		VTargetLocation.longitude+=0.1;	
 
 	print_string("basic hw checking\n");
 
	print_string("power on sensors\n");
 
	print_string("power on actuators\n");
 
	FProgNextConfig();	
 	  
}



	//*****Pools

	//CEDROOMPOOLCDLocation

Vehicle::EDROOMTopContext::CEDROOMPOOLCDLocation::CEDROOMPOOLCDLocation(TEDROOMUInt32 elemCount, CDLocation* pMem, TEDROOMBool * pMemMarks):CEDROOMProtectedMemoryPool(elemCount, pMem, pMemMarks, sizeof (CDLocation)){;}

CDLocation *	Vehicle::EDROOMTopContext::CEDROOMPOOLCDLocation::AllocData(){
	return(CDLocation*)CEDROOMProtectedMemoryPool::AllocData();
}



// ***********************************************************************

// class EDROOMTopState

// ***********************************************************************



	// CONSTRUCTOR*************************************************

Vehicle::EDROOMTopState::EDROOMTopState (Vehicle &act, CEDROOMMemory *pEDROOMMemory  ):
	  EDROOMTopContext( act 
		, VErrorInConfig
		, VTargetLocation
		, EDROOMPoolCDLocation )
	, EDROOMPoolCDLocation ( 10, pEDROOMMemory->poolCDLocation, pEDROOMMemory->poolMarkCDLocation)
{

}

	// EDROOMBehaviour**********************************************

void Vehicle::EDROOMTopState::EDROOMBehaviour(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans = EDROOMIllegada();

	do{

		switch(edroomCurrentTrans.localId){

			case(Init):
				edroomNextState = StartUp;
				break;
			case(ConfigAvObstacles):
				FConfigAvObstacles();

				EDROOMcomponent.EDROOMTraceStateEntry();

				if( VErrorInConfig ){
					FAvObstaclesError();
					edroomCurrentTrans.localId = ConfigAvObstacles_Error;
					edroomNextState = SysRecovery;
				 } 
				else{
					edroomCurrentTrans.localId = ConfigAvObstacles_OK;
					edroomNextState = AvObstaclesConfigured;
				 } 
				EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);
				break;
			case(ReStart):
				edroomNextState = StartUp;
				break;
			case(ConfigPathTracker):
				FConfigPathTracker();

				EDROOMcomponent.EDROOMTraceStateEntry();

				if( VErrorInConfig ){
					FPathTrackerError();
					edroomCurrentTrans.localId = ConfigPathTracker_Error;
					edroomNextState = SysRecovery;
				 } 
				else{
					edroomCurrentTrans.localId = ConfigPathTracker_OK;
					edroomNextState = PathTrackerConfigured;
				 } 
				EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);
				break;
			case(ConfigPathPlanner):
				FConfigPathPlanner();

				EDROOMcomponent.EDROOMTraceStateEntry();

				if( VErrorInConfig ){
					FPathPlannerError();
					edroomCurrentTrans.localId = ConfigPathPlanner_Error;
					edroomNextState = SysRecovery;
				 } 
				else{
					FStart();
					edroomCurrentTrans.localId = ConfigPathPlanner_OK;
					edroomNextState = VehicleReady;
				 } 
				EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);
				break;
			case(ObstacleDetected):
				FHandleObstacle();
				edroomNextState = VehicleReady;
				break;
		}

		EDROOMcomponent.EDROOMTraceStateEntry();

		EDROOMcomponent.EDROOMTraceStateEntry();

		switch(edroomNextState){

			case(I):
				edroomCurrentTrans=EDROOMIllegada();
				break;

			case(StartUp):
				FStartUp();
				edroomCurrentTrans=EDROOMStartUpllegada();
				break;

			case(VehicleReady):
				edroomCurrentTrans=EDROOMVehicleReadyllegada();
				break;

			case(SysRecovery):
				FRecovery();
				edroomCurrentTrans=EDROOMSysRecoveryllegada();
				break;

			case(AvObstaclesConfigured):
				FProgNextConfig();
				edroomCurrentTrans=EDROOMAvObstaclesConfiguredllegada();
				break;

			case(PathTrackerConfigured):
				FProgNextConfig();
				edroomCurrentTrans=EDROOMPathTrackerConfiguredllegada();
				break;

		}

		EDROOMcomponent.EDROOMTraceStateExit();

		edroomCurrentState=edroomNextState;

	}while(Msg->signal != EDROOMSignalDestroy);

}



	// Inicio**********************************************

void Vehicle::EDROOMTopState::EDROOMInicio(){

edroomCurrentState=I;

}



//	 ***********************************************************************

//	 llegada SubEstado I

//	 ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMIllegada(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans.localId = Init;
	edroomCurrentTrans.distanceToContext = 0;
	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, 0);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja StartUp

	// ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMStartUpllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = ConfigAvObstacles;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja VehicleReady

	// ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMVehicleReadyllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (SObstacleDetected): 

				 if ( *Msg->GetPInterface() == AvoidObstaclesSysCtrl) {

					edroomCurrentTrans.localId= ObstacleDetected;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja SysRecovery

	// ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMSysRecoveryllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja AvObstaclesConfigured

	// ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMAvObstaclesConfiguredllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = ConfigPathTracker;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja PathTrackerConfigured

	// ***********************************************************************



TEDROOMTransId Vehicle::EDROOMTopState::EDROOMPathTrackerConfiguredllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = ConfigPathPlanner;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



